/*
 * earsHandler.cpp
 *
 *  Created on: Nov 27, 2012
 *      Author: michiel
 */

#include "emotion/ears_handler.h"

EarsHandler::EarsHandler()
{
	_ears_pub = nh_.advertise<comm::ears>(Topic::MOTOR_EARS, 1);
}

EarsHandler::~EarsHandler()
{
}

/**
 * Publish ears state (emotion).
 */
void EarsHandler::publish()
{
	_ears_pub.publish(ears_state_);
}

void EarsHandler::init() {
	// lol empty!
}

void EarsHandler::spin() {
	ros::Rate loop_rate(30);

	while (ros::ok()) {
		if (emoteOk()) {
			processEmotion();
			publish();
		}

		ros::spinOnce();
		loop_rate.sleep();
	}
}

/**
 * Set the state of the ears according to the emotion.
 */
void EarsHandler::processEmotion() {
	switch (getEmotion()) {
	case EMOTION_HAPPY:
		setEarsState(EARS_FLAP);
		break;
	case EMOTION_BORED:
		setEarsState(EARS_REAR);
		break;
	case EMOTION_SAD:
		setEarsState(EARS_FRONT);
		break;
	default:
		setEarsState(EARS_NEUTRAL);
		setEarsAngle(0.5);
		break;
	}
}

/**
 * Set the left ear angle.
 */
void EarsHandler::setLEarState(int state)
{
	ears_state_.leftstate = state;
}

/**
 * Set the right ear state.
 */
void EarsHandler::setREarState(int state)
{
	ears_state_.rightstate = state;
}

/**
 * Set the state of both the ears.
 */
void EarsHandler::setEarsState(int state)
{
	setLEarState(state);
	setREarState(state);
}

/**
 * Set left ear angle (used for rviz).
 */
void EarsHandler::setLEarAngle(float angle)
{
	ears_state_.leftangle = angle;
}

/**
 * Set right ear angle (used for rviz).
 */
void EarsHandler::setREarAngle(float angle)
{
	ears_state_.rightangle = angle;
}

/**
 * Set the angle of both the ears (used for rviz).
 */
void EarsHandler::setEarsAngle(float angle)
{
	setLEarAngle(angle);
	setREarAngle(angle);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "earsHandlerEmotion");

	EarsHandler handler;
	handler.init();
	handler.spin();
}
